#ifndef __BSP_MOTOR_PID_H
#define __BSP_MOTOR_PID_H

#include "bsp_can.h"
#include <stdint.h>

// PID控制模式
typedef enum {
    CONTROL_MODE_VELOCITY = 0,  // 速度控制模式
    CONTROL_MODE_POSITION = 1,  // 位置控制模式
    CONTROL_MODE_CURRENT = 2    // 直接电流控制模式
} pid_control_mode_t;

// PID调试信息结构
typedef struct {
    float output;        // 输出值
    float error;         // 误差值
    float p_term;        // 比例项
    float i_term;        // 积分项  
    float d_term;        // 微分项
    uint32_t update_count; // 更新计数
} pid_debug_info_t;

// 初始化函数
void motor_pid_init(void);

// 控制模式设置
void motor_pid_set_mode(pid_control_mode_t mode);

// 单个电机控制
int16_t motor_pid_velocity_control(motor_id_t motor, float target_rpm);
int16_t motor_pid_position_control(motor_id_t motor, float target_deg);

// 所有电机控制
void motor_pid_all_velocity_control(float rpm_a, float rpm_b, float rpm_c, float rpm_d);
void motor_pid_all_position_control(float deg_a, float deg_b, float deg_c, float deg_d);

// 麦轮控制
void motor_pid_mecanum_velocity(float vx, float vy, float vz);

// PID管理
void motor_pid_reset_all(void);

// 调试信息
void motor_pid_get_debug_info(motor_id_t motor, pid_debug_info_t *info);

// ==================== 调试参数设置接口 ====================
// 调试时可以调用这些函数实时修改PID参数

// 设置速度环PID参数
void motor_pid_set_velocity_params(motor_id_t motor, float kp, float ki, float kd);

// 设置位置环PID参数  
void motor_pid_set_position_params(motor_id_t motor, float kp, float ki, float kd);

// 设置所有电机速度环PID参数
void motor_pid_set_all_velocity_params(float kp, float ki, float kd);

// 设置所有电机位置环PID参数
void motor_pid_set_all_position_params(float kp, float ki, float kd);

// 获取当前PID参数
void motor_pid_get_velocity_params(motor_id_t motor, float *kp, float *ki, float *kd);
void motor_pid_get_position_params(motor_id_t motor, float *kp, float *ki, float *kd);

#endif